#pragma once
#include "ReusableIndirectHeap.h"
#include "PathFindingAlgorithm.h"



class AStarStaticMemory :public PathFindingAlgorithm{
public:
	bool postSmoothingOn = false;
	bool isrepeatedPostSmooth = false;
	float heuristicWeight = 1.f;

	ReusableIndirectHeap pq;

	int finish;


	AStarStaticMemory(GridGraph graph, int sx, int sy, int ex, int ey) 
		:PathFindingAlgorithm(graph, graph.sizeX, graph.sizeY, sx, sy, ex, ey)
	{

	}

	static AStarStaticMemory postSmooth(GridGraph graph, int sx, int sy, int ex, int ey) {
		AStarStaticMemory aStar(graph, sx, sy, ex, ey);
		aStar.postSmoothingOn = true;
		aStar.isrepeatedPostSmooth = false;
		return aStar;
	}

	static AStarStaticMemory repeatedPostSmooth(GridGraph graph, int sx, int sy, int ex, int ey) {
		AStarStaticMemory aStar(graph, sx, sy, ex, ey);
		aStar.postSmoothingOn = true;
		aStar.isrepeatedPostSmooth = true;
		return aStar;
	}

	static AStarStaticMemory dijkstra(GridGraph graph, int sx, int sy, int ex, int ey) {
		AStarStaticMemory aStar(graph, sx, sy, ex, ey);
		aStar.heuristicWeight = 0;
		return aStar;
	}

	void computePath() {
			int totalSize = (graph.sizeX + 1) * (graph.sizeY + 1);

			int start = toOneDimIndex(sx, sy);
			finish = toOneDimIndex(ex, ey);

 			pq =ReusableIndirectHeap(totalSize);
			initialiseMemory(totalSize, FLT_MAX, -1, false);

			initialise(start);

			float lastDist = -1;
			while (!pq.isEmpty()) {
				float dist = pq.getMinValue();

				int current = pq.popMinIndex();

				//if (Math.abs(dist - lastDist) > 0.01f) { maybeSaveSearchSnapshot(); lastDist = dist;}
				//maybeSaveSearchSnapshot();

				if (current == finish || distance(current) ==FLT_MAX) {
					//maybeSaveSearchSnapshot();
					break;
				}
				setVisited(current, true);

				int x = toTwoDimX(current);
				int y = toTwoDimY(current);

				//1 2 3
				tryRelaxNeighbour(current, x, y, x - 1, y - 1);
				tryRelaxNeighbour(current, x, y, x, y - 1);
				tryRelaxNeighbour(current, x, y, x + 1, y - 1);

				//4 6
				tryRelaxNeighbour(current, x, y, x - 1, y);
				tryRelaxNeighbour(current, x, y, x + 1, y);

				//7 8 9
				tryRelaxNeighbour(current, x, y, x - 1, y + 1);
				tryRelaxNeighbour(current, x, y, x, y + 1);
				tryRelaxNeighbour(current, x, y, x + 1, y + 1);

				//maybeSaveSearchSnapshot();
			}

			maybePostSmooth();
		}

	virtual void tryRelaxNeighbour(int current, int currentX, int currentY, int x, int y) {
		if (!graph.isValidCoordinate(x, y))
			return;

		int destination = toOneDimIndex(x, y);
		if (visited(destination))
			return;
		if (graph.isBlocked(x, y))
			return;
		if (!graph.neighbourLineOfSight(currentX, currentY, x, y)) 
			return;

		if (relax(current, destination, weight(currentX, currentY, x, y))) {
			// If relaxation is done.
			pq.decreaseKey(destination, distance(destination) + heuristic(x, y));
		}
	}

	float heuristic(int x, int y) {
		//return 0;
		return heuristicWeight*graph.distance(x, y, ex, ey);
	}


	float weight(int x1, int y1, int x2, int y2) {
		return graph.distance(x1, y1, x2, y2);
	}

	virtual bool relax(int u, int v, float weightUV) {
		// return true iff relaxation is done.

		float newWeight = distance(u) + weightUV;
		if (newWeight < distance(v)) {
			setDistance(v, newWeight);
			setParent(v, u);
			//maybeSaveSearchSnapshot();
			return true;
		}
		return false;
	}


	void initialise(int s) {
		pq.decreaseKey(s, 0.0f);
		Memory::setDistance(s, 0.0f);
	}


	int PathLength() {
		int length = 0;
		int current = finish;
		while (current != -1) {
			current = parent(current);
			length++;
		}
		return length;
	}

	bool lineOfSight(int node1, int node2) {
		int x1 = toTwoDimX(node1);
		int y1 = toTwoDimY(node1);
		int x2 = toTwoDimX(node2);
		int y2 = toTwoDimY(node2);
		return graph.lineOfSight(x1, y1, x2, y2);
	}

	float physicalDistance(int node1, int node2) {
		int x1 = toTwoDimX(node1);
		int y1 = toTwoDimY(node1);
		int x2 = toTwoDimX(node2);
		int y2 = toTwoDimY(node2);
		return graph.distance(x1, y1, x2, y2);
	}

	float physicalDistance(int x1, int y1, int node2) {
		int x2 = toTwoDimX(node2);
		int y2 = toTwoDimY(node2);
		return graph.distance(x1, y1, x2, y2);
	}

	void maybePostSmooth() {
		if (postSmoothingOn) {
			if (isrepeatedPostSmooth) {
				while (postSmooth());
			}
			else {
				postSmooth();
			}
		}
	}

	bool postSmooth() {
		bool didSomething = false;

		int current = finish;
		while (current != -1) {
			int next = parent(current); // we can skip checking this one as it always has LoS to current.
			if (next != -1) {
				next = parent(next);
				while (next != -1) {
					if (lineOfSight(current, next)) {
						setParent(current, next);
						next = parent(next);
						didSomething = true;
						//maybeSaveSearchSnapshot();
					}
					else {
						next = -1;
					}
				}
			}

			current = parent(current);
		}

		return didSomething;
	}


	std::vector<std::pair<int,int>> getPath() {
		//std::vector<std::pair<int, int>> vec;
		//return vec;
		int length = PathLength();

		std::vector<std::pair<int, int>> path;
		path.resize(length);
		//int[][] path = new int[length][];

		int current = finish;

		int index= length - 1;
		while (current != -1) {
			int x = toTwoDimX(current);
			int y = toTwoDimY(current);

			//path[index] = new int[2];
			//path[index][0] = x;
			//path[index][1] = y;
			path[index].first = x;
			path[index].second = y;
			index--;
			current = parent(current);
		}

		return path;
	}

	float getPathLength() {
		int current = finish;
		if (current == -1) return -1;

		float pathLength = 0;

		int prevX = toTwoDimX(current);
		int prevY = toTwoDimY(current);
		current = parent(current);

		while (current != -1) {
			int x = toTwoDimX(current);
			int y = toTwoDimY(current);

			pathLength += graph.distance(x, y, prevX, prevY);

			current = parent(current);
			prevX = x;
			prevY = y;
		}

		return pathLength;
	}

		bool selected(int index) {
			return visited(index);
		}


	int parent(int index) {
		return Memory::Getparent(index);
	}

	void setParent(int index, int value) {
		Memory::setParent(index, value);
	}

	float distance(int index) {
		return Memory::Getdistance(index);
	}

	void setDistance(int index, float value) {
		Memory::setDistance(index, value);
	}

	bool visited(int index) {
		return Memory::Getvisited(index);
	}

	void setVisited(int index, bool value) {
		Memory::setVisited(index, value);
	}
};